/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : kal_uwb_flow.hpp
  * Description        :
  ******************************************************************************
  * Function           :
  *
  ******************************************************************************
  * Author             : Yuyuan Huang
  * Creation Date      : Sep 16, 2022
  ******************************************************************************
  */
/* USER CODE END Header */

#ifndef __KAL_UWB_FLOW_H
#define __KAL_UWB_FLOW_H

#include "system/system.hpp"
#include "userlib/userlib.hpp"


#ifdef __cplusplus
extern "C" {
#endif

//#define SLIDE_WINDOW_SIZE 10
//
//#define UWB_MAX_LIMIT 1.0
//#define UWB_MIN_LIMIT 0.1

//采集频率 200hz
//截至频率 10hz
//#define uwb_a0 1.0f
//#define uwb_a1 -2.462221601085747f
//#define uwb_a2 2.059628602622409f
//#define uwb_a3 -0.582169043324628f
//
//#define uwb_b0 0.001904744776504f
//#define uwb_b1 0.005714234329513f
//#define uwb_b2 0.005714234329513f
//#define uwb_b3 0.001904744776504f

#ifdef __cplusplus
}
#endif


#ifdef __cplusplus

#include <Eigen>

using namespace std;
using namespace Eigen;

class KALUWBFLOW
{
public:
	KALUWBFLOW(){}
	KALUWBFLOW(char *n) : name(n){}
	~KALUWBFLOW(){}
	void KalUwbFlow_Init();
	void KalUwbFlow_Pred();
	void KalUwbFlow_Update();
//	void slideUwb_Init();
//	void slideUwb_Update();
//	void butterworth_init();
//	void butterworth_update();

private:
	char *name;
	sTIM Tim;
	bool Update;
	uint32_t startTimer;
	uint32_t stopTimer;
	uint32_t  executionTime_us;
	uint32_t startTimerLast;
	uint32_t cycleTime_us;

	Quaternion<float> q;

	//矩阵运算变量定义（KAL）
	Matrix<float,6,9> H;		//灵敏度矩阵
	Matrix<float,9,6> K;		//kalman gain
	Matrix<float,9,9> P_p;		//协方差矩阵预测
	Matrix<float,9,9> P_k;		//协方差矩阵更新
	Matrix<float,6,6> R;		//测量噪声
//	Matrix<float,6,6> Q;		//过程噪声
	Matrix<float,9,9> Q;		//过程噪声
	Matrix<float,6,1> y;					//测量值
	Matrix<float,9,1> X_p;		//状态预测
	Matrix<float,9,1> X_k;		//状态更新
	Matrix<float,9,9> F;
	Matrix<float,9,3> B;
	Matrix<float,9,6> G;

	Matrix<float,3,3> Cb2n;	//机体坐标系到NED的旋转矩阵

	Vector3f a_k;			//机体坐标系下的加速度
	Vector3f accUWB;
	Vector3f accUWBTemp;
	Matrix3f Rz;			//NED转UWB坐标系旋转矩阵
	Vector3f u_k;			//UWB坐标系下XY轴加速度
	float dt;
	float yaw;

	sensor_acc_msg acc;
	ahrs_euler_msg ahrsEuler;
	uwb_pos_msg uwbPos;
	kal_uwb_pos_vel_msg kal_uwb_pos_vel;
	Control_msg control_msg;
	flow_msg flow;
	height_msg height;

	/* slide window filter */
//	float slide_window_x[SLIDE_WINDOW_SIZE];
//	float slide_window_y[SLIDE_WINDOW_SIZE];
//	float coe_x, coe_y;
//	int sw_cnt;
//	float sw_output_x, sw_output_y;
//	float sum_x, sum_y;
//	int init_cnt;
	/* slide window filter */

	/* butterworth */
//	float A_last_3nd[3];
//	float A_last_last_3nd[3];
//	float A_last_last_last_3nd[3];
//	float B_last_3nd[3];
//	float B_last_last_3nd[3];
//	float B_last_last_last_3nd[3];
//	float UwbFil_3nd[3];//3阶低通滤波器
	/* butterworth */
};

#endif


#endif


/************************ (C) COPYRIGHT Longmen Drone Team *****END OF FILE****/
